package org.lejos.example;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;

/**
 * Example leJOS Project with an ant build file
 *
 */
public class HelloWorld {


    public static final String GO_FORWARD_COMMAND = "forward";
    public static final String GO_LEFT_COMMAND = "left";
    public static final String GO_RIGHT_COMMAND = "right";


    public static String padString(String string,int size)
    {



        int strLen = string.length();

        if(strLen >= size)
        {
            return string;
        }
        else
        {

            int padding = size - strLen;

            for(int i=0;i<padding;i++)
            {
                string = string + " ";
            }

            return string;


        }

    }

    public static void main(String[] args) {


        String connected = "Connected";
        String waiting = "Waiting...";
        String closing = "Closing...";

        while (true) {
            LCD.drawString(waiting, 0, 0);
            LCD.refresh();

            BTConnection btc = Bluetooth.waitForConnection();
            //USBConnection btc = USB.waitForConnection();


            LCD.clear();
            LCD.drawString(connected, 0, 0);
            LCD.refresh();

            try {
                System.out.println("Creating streams");
                DataInputStream dis = btc.openDataInputStream();
                DataOutputStream dos = btc.openDataOutputStream();
                System.out.println("Created streams");
                Thread.sleep(2000);

                System.out.println("Reading streams...");

                byte[] data = new byte[1024];

                while (!Button.LEFT.isPressed()) {
                    //System.out.println("Reading int");
                    //int size = dis.readInt();
                    int size = dis.read(data);

                    if (size > -1) {

                        System.out.println("Got data - size:" + size);
                        String utf8SrcString = new String(data, 0, size, "UTF8");//in string
                        System.out.println(utf8SrcString);


                        if(utf8SrcString.equals(GO_FORWARD_COMMAND))
                        {
                            System.out.println("going forwards");
                            Motor.A.forward();
                            Motor.C.forward();
                            Thread.sleep(1000);
                            Motor.A.stop();
                            Motor.C.stop();
                        }
                        else if(utf8SrcString.equals(GO_LEFT_COMMAND))
                        {
                            System.out.println("going left");
                            Motor.A.backward();
                            Motor.C.forward();
                            Thread.sleep(1000);
                            Motor.A.stop();
                            Motor.C.stop();
                        }
                        else if(utf8SrcString.equals(GO_RIGHT_COMMAND))
                        {
                            System.out.println("going right");
                            Motor.A.forward();
                            Motor.C.backward();
                            Thread.sleep(1000);
                            Motor.A.stop();
                            Motor.C.stop();
                        }
                        else{
                            System.out.println("Unknown command");
                        }


                        System.out.println("Attempting to echo message back...");
                        Thread.sleep(1000);


                        utf8SrcString = utf8SrcString + " - NXT ";

                        utf8SrcString = padString(utf8SrcString,50);

                        System.out.println(utf8SrcString);


                        try {
                            byte[] result = utf8SrcString.getBytes("UTF8");//in byte array
                            dos.write(result);
                            dos.flush();
                        } catch (Exception e) {
                            System.out.println("Error encoding string! - " + e.toString());
                        }

                    } else {
                        System.out.println("Received noise");
                    }


                    Thread.sleep(100);
                    String utf8SrcString = "Hello PC from NXT ";

//                        try {
//                            System.out.println("Sending message.");
//                            byte[] result = utf8SrcString.getBytes("UTF8");//in byte array
//                            dos.write(result);
//                            dos.flush();
//                        } catch (Exception e) {
//                            System.out.println("Error encoding string! - " + e.toString());
//                        }
                }

                System.out.println("Closing streams");


                dis.close();
                dos.close();
                Thread.sleep(100); // wait for data to drain
                LCD.clear();
                LCD.drawString(closing, 0, 0);
                LCD.refresh();
                btc.close();
                LCD.clear();
            } catch (Exception e) {
                System.out.println(e.toString());
            }
        }
    }
}


